home *** CD-ROM | disk | FTP | other *** search
/ Space & Astronomy / Space and Astronomy (October 1993).iso / mac / programs / satrack / SS4EXE.ZIP / SEESAT4.ZIP / SGP4.C < prev    next >
Text File  |  1992-01-05  |  9KB  |  301 lines
  1. /*
  2. SGP4.C
  3. by Paul S. Hirose, 1991 Oct 19
  4. Orbit prediction model for the SEESAT satellite tracking program.
  5.  
  6. This file is in the public domain.
  7.  
  8. 300 - 399
  9. */
  10.  
  11. /* library functions used in this file:
  12.     atan2 cos sin sqrt
  13. */
  14.  
  15. #include "b:seesat.h"    /* the global header */
  16.  
  17. #if ECOC
  18. extern double atan2(), cos(), sin(), sqrt();
  19. #endif
  20.  
  21. /* If true, BSTAR will be estimated if a value is not provided in the element
  22. set.  Estimation formula courtesy Ted Molczan. */
  23. #define ESTB 1
  24.  
  25. /* NORAD velocity code is disabled because it's not used in SEESAT.  Can be
  26. activated by setting VEL nonzero and defining an output struct for the
  27. velocities as follows (struct tag "vector" is defined in SEESAT.H):
  28. struct vector satdot;
  29. */
  30.  
  31. #define VEL 0
  32.  
  33. static double
  34. a, ao, aodp, axn, aycof, ayn, aynl, a1, a3ovk2, beta, betal, betao,
  35. betao2, capu, coef, coef1, cosepw, cosik, cosio, cosnok, cosu, cosuk,
  36. cos2u, c1, c1sq, c2, c3, c4, c5, delm, delmo, delo, delomg, del1, d2,
  37. d3, d4, e, ecose, eeta, elsq, eosq, epw, esine, eta, etasq, omega,
  38. omgcof, omgadf, omgdot, perige, pinvsq, pl, psisq, qoms24, r, rdot,
  39. rdotk, rfdot, rfdotk, rk, sinepw, sinik, sinio, sinmo, sinnok, sinu,
  40. sinuk, sin2u, s4, tcube, temp, tfour, tempa, tempe, templ, temp1,
  41. temp2, temp3, temp4, temp5, temp6, theta2, theta4, tsi, tsince, tsq,
  42. t2cof, t3cof, t4cof, t5cof, u, uk, ux, uy, uz, vx, vy, vz, xhdot1,
  43. xinck, xl, xlcof, xll, xlt, xmcof, xmdf, xmdot, xmp, xmx, xmy, xn,
  44. xnodcf, xnoddf, xnode, xnodek, xnodot, xnodp, x1cof, x1mth2, x1m5th,
  45. x3thm1, x7thm1;
  46.  
  47. static int i, isimp;
  48.  
  49. void
  50. sgp4(tsince)
  51. double tsince;
  52. {
  53.     DTEST((300, 1, &iflag));
  54.     FTEST((301, 1, 3, &tsince));
  55.  
  56.     if (iflag) {
  57.         /* this is first call of sgp4() in prediction run */
  58.  
  59.         /* Recover original mean motion (xnodp) and semimajor axis
  60.         (aodp) from input elements. */
  61.  
  62.         a1 = POW(xke / xno, tothrd);
  63.         cosio = cos(xincl);
  64.         theta2 = cosio * cosio;
  65.         x3thm1 = 3. * theta2 - 1.;
  66.         eosq = eo * eo;
  67.         betao2 = 1. - eosq;
  68.         betao = sqrt(betao2);
  69.         del1 = 1.5 * ck2 * x3thm1 / (a1 * a1 * betao * betao2);
  70.         ao = a1 * (1. - del1 * (.5 * tothrd + del1 * (1. + 134.
  71.          / 81. * del1)));
  72.         delo = 1.5 * ck2 * x3thm1 / (ao * ao * betao * betao2);
  73.         xnodp = xno / (1. + delo);
  74.         aodp = ao / (1. - delo);
  75.  
  76.         /* Initialization.  For perigee less than 220 km, the
  77.         "isimp" flag is set and the equations are truncated to linear
  78.         variation in sqrt(a) and quadratic variation in mean anomaly.
  79.         Also, the c3, delta omega, and delta m terms are dropped. */
  80.  
  81.         if (aodp * (1. - eo) <  220. / xkmper + 1.)
  82.             isimp = 1;
  83.         else
  84.             isimp = 0;
  85.  
  86.         s4 = s;
  87.         qoms24 = qoms2t;
  88.         perige = (aodp * (1. - eo) - 1.) * xkmper;
  89.  
  90.         /* For perigee below 156 km, the values of
  91.         s and qoms2t are altered. */
  92.         if (perige < 156.) {
  93.             if (perige > 98.)
  94.                 s4 = perige - 78.;
  95.             else
  96.                 s4 = 20.;
  97.             qoms24 = POW((120. - s4) / xkmper, 4.);
  98.             s4 = s4 / xkmper + 1.;
  99.         }
  100.         pinvsq = 1. / (aodp * aodp * betao2 * betao2);
  101.         tsi = 1. / (aodp - s4);
  102.         eta = aodp * eo * tsi;
  103.         etasq = eta * eta;
  104.         eeta = eo * eta;
  105.         psisq = FABS(1. - etasq);
  106.         coef = qoms24 * POW(tsi, 4.);
  107.         coef1 = coef / POW(psisq, 3.5);
  108.         c2 = coef1 * xnodp * (aodp * (1. + 1.5 * etasq + eeta *
  109.          (4. + etasq)) + .75 * ck2 * tsi / psisq * x3thm1 *
  110.          (8. + 3. * etasq * (8. + etasq)));
  111. #if ESTB
  112.         if (bstar == 0.)    /* field in element set was blank */
  113.             bstar = tothrd * xndt2o / (xno * c2);
  114.         ETEST((305, 1, 2, &bstar));
  115. #endif
  116.         c1 = bstar * c2;
  117.         sinio = sin(xincl);
  118.         a3ovk2 = -xj3 / ck2;
  119.         c3 = coef * tsi * a3ovk2 * xnodp * sinio / eo;
  120.         x1mth2 = 1. - theta2;
  121.         c4 = 2. * xnodp * coef1 * aodp * betao2 * (eta
  122.          * (2. + .5 * etasq) + eo * (.5 + 2. * etasq) - 2. * ck2
  123.          * tsi / (aodp * psisq) * (-3. * x3thm1 * (1. - 2. * eeta
  124.          + etasq * (1.5 -.5 * eeta)) + .75 * x1mth2 * (2. * etasq
  125.          - eeta * (1. + etasq)) * cos(2. * omegao)));
  126.         c5 = 2. * coef1 * aodp * betao2 * (1. + 2.75 * (etasq + eeta)
  127.          + eeta * etasq);
  128.         theta4 = theta2 * theta2;
  129.         temp1 = 3. * ck2 * pinvsq * xnodp;
  130.         temp2 = temp1 * ck2 * pinvsq;
  131.         temp3 = 1.25 * ck4 * pinvsq * pinvsq * xnodp;
  132.         xmdot = xnodp + .5 * temp1 * betao * x3thm1 + .0625 * temp2
  133.          * betao * (13. - 78. * theta2 + 137. * theta4);
  134.         x1m5th = 1. - 5. * theta2;
  135.         omgdot = -.5 * temp1 * x1m5th + .0625 * temp2 * (7. - 114.
  136.          * theta2 + 395. * theta4) + temp3 * (3. - 36. * theta2
  137.          + 49. * theta4);
  138.         xhdot1 = -temp1 * cosio;
  139.         xnodot = xhdot1 + (.5 * temp2 * (4. - 19. * theta2) + 2.
  140.          * temp3 * (3. - 7. * theta2)) * cosio;
  141.         omgcof = bstar * c3 * cos(omegao);
  142.         xmcof = -tothrd * coef * bstar / eeta;
  143.         xnodcf = 3.5 * betao2 * xhdot1 * c1;
  144.         t2cof = 1.5 * c1;
  145.         xlcof = .125 * a3ovk2 * sinio * (3. + 5. * cosio)
  146.          / (1. + cosio);
  147.         aycof = .25 * a3ovk2 * sinio;
  148.         delmo = POW(1. + eta * cos(xmo), 3.);
  149.         sinmo = sin(xmo);
  150.         x7thm1 = 7. * theta2 - 1.;
  151.         if (!isimp) {
  152.             c1sq = c1 * c1;
  153.             d2 = 4. * aodp * tsi * c1sq;
  154.             temp = d2 * tsi * c1 / 3.;
  155.             d3 = (17. * aodp + s4) * temp;
  156.             d4 = .5 * temp * aodp * tsi
  157.              * (221. * aodp + 31. * s4) * c1;
  158.             t3cof = d2 + 2. * c1sq;
  159.             t4cof = .25 * (3. * d3 + c1 * (12. * d2 + 10.
  160.              * c1sq));
  161.             t5cof = .2 * (3. * d4 + 12. * c1 * d3 + 6. * d2 * d2
  162.              + 15. * c1sq * (2. * d2 + c1sq));
  163.         } iflag = 0;
  164.     }
  165.  
  166.     /* update for secular gravity and atmospheric drag */
  167.  
  168.     xmdf = xmo + xmdot * tsince;
  169.     omgadf = omegao + omgdot * tsince;
  170.     xnoddf = xnodeo + xnodot * tsince;
  171.     omega = omgadf;
  172.     xmp = xmdf;
  173.     tsq = tsince * tsince;
  174.     xnode = xnoddf + xnodcf * tsq;
  175.     tempa = 1. - c1 * tsince;
  176.     tempe = bstar * c4 * tsince;
  177.     templ = t2cof * tsq;
  178.     if (!isimp) {
  179.         delomg = omgcof * tsince;
  180.         delm = xmcof * (POW(1. + eta * cos(xmdf), 3.) - delmo);
  181.         temp = delomg + delm;
  182.         xmp = xmdf + temp;
  183.         omega = omgadf - temp;
  184.         tcube = tsq * tsince;
  185.         tfour = tsince * tcube;
  186.         tempa = tempa - d2 * tsq - d3 * tcube - d4 * tfour;
  187.         tempe = tempe + bstar * c5 * (sin(xmp) - sinmo);
  188.         templ = templ + t3cof * tcube + tfour * (t4cof + tsince
  189.          * t5cof);
  190.     } a = aodp * tempa * tempa;
  191.     e = eo - tempe;
  192.     xl = xmp + omega + xnode + xnodp * templ;
  193.     beta = sqrt(1. - e * e);
  194.     xn = xke / POW(a, 1.5);
  195.  
  196.     /* long period periodics */
  197.  
  198.     axn = e * cos(omega);
  199.     temp = 1. / (a * beta * beta);
  200.     xll = temp * xlcof * axn;
  201.     aynl = temp * aycof;
  202.     xlt = xl + xll;
  203.     ayn = e * sin(omega) + aynl;
  204.  
  205.     /* solve Kepler's equation */
  206.  
  207.     capu = fmod2p(xlt - xnode);
  208.  
  209.     FTEST((302, 3, 3, &xlt, &xnode, &capu));
  210.  
  211.     temp2 = capu;
  212.     for (i = -10; i; ++i) {
  213.         sinepw = sin(temp2);
  214.         cosepw = cos(temp2);
  215.         temp3 = axn * sinepw;
  216.         temp4 = ayn * cosepw;
  217.         temp5 = axn * cosepw;
  218.         temp6 = ayn * sinepw;
  219.         epw = (capu - temp4 + temp3 - temp2) / (1. - temp5 - temp6)
  220.          + temp2;
  221.         if (FABS(epw - temp2) <= e6a)
  222.             break;
  223.         temp2 = epw;
  224.     }
  225.     DTEST((306, 1, &i));
  226.  
  227.     /* short period preliminary quantities */
  228.  
  229.     ecose = temp5 + temp6;
  230.     esine = temp3 - temp4;
  231.     elsq = axn * axn + ayn * ayn;
  232.     temp = 1. - elsq;
  233.     pl = a * temp;
  234.     r = a * (1. - ecose);
  235.     temp1 = 1. / r;
  236. #if VEL
  237.     rdot = xke * sqrt(a) * esine * temp1;
  238.     rfdot = xke * sqrt(pl) * temp1;
  239. #endif
  240.     temp2 = a * temp1;
  241.     betal = sqrt(temp);
  242.     temp3 = 1. / (1. + betal);
  243.     cosu = temp2 * (cosepw - axn + ayn * esine * temp3);
  244.     sinu = temp2 * (sinepw - ayn - axn * esine * temp3);
  245.     u = atan2(sinu, cosu);
  246.     sin2u = 2. * sinu * cosu;
  247.     cos2u = 2. * cosu * cosu - 1.;
  248.     temp = 1. / pl;
  249.     temp1 = ck2 * temp;
  250.     temp2 = temp1 * temp;
  251.  
  252.     /* update for short periodics */
  253.  
  254.     rk = r * (1. - 1.5 * temp2 * betal * x3thm1) + .5 * temp1 * x1mth2
  255.      * cos2u;
  256.     uk = u - .25 * temp2 * x7thm1 * sin2u;
  257.     xnodek = xnode + 1.5 * temp2 * cosio * sin2u;
  258.     xinck = xincl + 1.5 * temp2 * cosio * sinio * cos2u;
  259. #if VEL
  260.     rdotk = rdot - xn * temp1 * x1mth2 * sin2u;
  261.     rfdotk = rfdot + xn * temp1 * (x1mth2 * cos2u + 1.5 * x3thm1);
  262. #endif
  263.  
  264.     /* orientation vectors */
  265.  
  266.     sinuk = sin(uk);
  267.     cosuk = cos(uk);
  268.     sinik = sin(xinck);
  269.     cosik = cos(xinck);
  270.     sinnok = sin(xnodek);
  271.     cosnok = cos(xnodek);
  272.     xmx = -sinnok * cosik;
  273.     xmy = cosnok * cosik;
  274.     ux = xmx * sinuk + cosnok * cosuk;
  275.     uy = xmy * sinuk + sinnok * cosuk;
  276.     uz = sinik * sinuk;
  277.  
  278. #if VEL
  279.     vx = xmx * cosuk - cosnok * sinuk;
  280.     vy = xmy * cosuk - sinnok * sinuk;
  281.     vz = sinik * cosuk;
  282. #endif
  283.  
  284.     /* position */
  285.  
  286.     sat.x = rk * ux;
  287.     sat.y = rk * uy;
  288.     sat.z = rk * uz;
  289.  
  290. #if VEL
  291.     satdot.x = rdotk * ux + rfdotk * vx;
  292.     satdot.y = rdotk * uy + rfdotk * vy;
  293.     satdot.z = rdotk * uz + rfdotk * vz;
  294. #endif
  295.  
  296.     FTEST((303, 3, 6, &sat.x, &sat.y, &sat.z));
  297.     FTEST((304, 3, 6, &satdot.x, &satdot.y, &satdot.z));
  298. }
  299.  cos2u;
  300.     uk = u - .25 * temp2 * x7thm1 * sin2u;
  301.     xnodek =